#include "Controller.h"

volatile uint8_t Flag_ExmNum = 0;   //比赛题目标志位

volatile uint8_t CReset_Flag = 0;   //8s标志位
volatile uint8_t Contrl_Flag = 0;
volatile float YAW_Data = 0;


//MPU零飘误差调整
void vMPU_Calibration(void)
{
    static float Temp_Err = 0;
    static uint8_t Flag = 0;

    if (CReset_Flag == 1 && pitch > -3.0 && pitch < 3.0 && Flag == 0) 
    {
        Temp_Err = yaw;
        Buzzer_Flag = 1;
        Flag = 1;
    }

    if(Flag == 1){
        YAW_Data = yaw - Temp_Err;
    }else if(Flag == 0){
        YAW_Data = yaw;
    }
}


void vMPU_Contrl(void)
{
    if(YAW_Data > -30 && YAW_Data < 30){
        if(YAW_Data > -5 && YAW_Data < 5){Motor_Control(Base_Value, Base_Value);}
        if(YAW_Data < -5 ){
            Motor_Control(-Base_Value, Base_Value);
        }
        if(YAW_Data > 5 ){
            Motor_Control(Base_Value, -Base_Value); 
        } 
    }else {
        if(YAW_Data > 175 && YAW_Data < 185 ){Motor_Control(Base_Value, Base_Value);}
        if(YAW_Data < 175 ){
            Motor_Control(-Base_Value, Base_Value); 
        }
        if(YAW_Data > 185 ){
            Motor_Control(-Base_Value, Base_Value); 
        } 
    }
}


void vBuzzer_Contrl(uint8_t State)
{
    if(State){
        DL_GPIO_setPins(Buzzer_PORT,Buzzer_OUT_PIN);
    }else{
        DL_GPIO_clearPins(Buzzer_PORT,Buzzer_OUT_PIN);
    }
}




